#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import rospy
import moveit_commander
import geometry_msgs.msg
from moveit_commander import RobotTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from geometry_msgs.msg import WrenchStamped
from sensor_msgs.msg import JointState
import time

# 用于控制是否暂停和恢复轨迹执行
FORCE_THRESHOLD = 2.0  # 力的阈值，单位：牛
force_exceeded = False  # 是否超过力的阈值
wrench_data = None  # 存储力的数据
first_check = False
pos = [0.0] * 5
joint = [0] * 5
wait_sign = False

def joint_state_callback(msg):
    global pos, joint, wait_sign
    if wait_sign:
        return
    # 获取 position 的大小
    len_positions = len(msg.position)
    
    # 只处理前 5 个关节的位置
    for j in range(min(len_positions, 5)):
        pos[j] = msg.position[j]
    if pos[2] > 2.6:
        print("juping")
        wait_sign = True

def wrench_callback(msg):
    """力数据的回调函数，判断是否超过力的阈值"""
    global wrench_data, force_exceeded
    wrench_data = msg.wrench.force
    # 判断任意一个方向上的力是否大于阈值
    if abs(wrench_data.x) > FORCE_THRESHOLD or abs(wrench_data.y) > FORCE_THRESHOLD or abs(wrench_data.z) > FORCE_THRESHOLD:
        force_exceeded = True
    else:
        force_exceeded = False

def get_new_start_pose(current_pose, poses):
    """
    判断 current_pose 所在的位置在 poses 中的哪个位置之间，并返回新的起始目标位姿。
    """
    for i in range(len(poses) - 1):
        pose_start = poses[i]
        pose_end = poses[i + 1]
        
        # 判断 current_pose 是否在 pose_start 和 pose_end 之间
        if (pose_start.position.x <= current_pose.position.x <= pose_end.position.x):
            # print("pose_start.position.x:", pose_start.position.x)
            # print("current_pose.position.x:", current_pose.position.x)
            # print("pose_end.position.x:", pose_end.position.x)
            # print(f"从第{i+1}个点开始") #因为有新的pose所以和原来pose的i对不上
            return poses[i+1:]
    # print("default")
    # 如果没有匹配的，就默认返回所有目标位姿
    return poses

def is_target_pose_reached(current_pose, target_pose, tolerance=0.01):
    """
    判断当前姿态与目标姿态是否接近，使用位置和姿态的容忍度。
    tolerance: 容忍度，单位米
    """
    position_reached = (
        abs(current_pose.position.x - target_pose.position.x) < tolerance and
        abs(current_pose.position.y - target_pose.position.y) < tolerance and
        abs(current_pose.position.z - target_pose.position.z) < tolerance
    )
    
    orientation_reached = (
        abs(current_pose.orientation.x - target_pose.orientation.x) < tolerance and
        abs(current_pose.orientation.y - target_pose.orientation.y) < tolerance and
        abs(current_pose.orientation.z - target_pose.orientation.z) < tolerance and
        abs(current_pose.orientation.w - target_pose.orientation.w) < tolerance
    )

    return position_reached and orientation_reached

def scale_trajectory_speed(traj, scale):
    # 创建新的轨迹对象并初始化
    new_traj = RobotTrajectory()
    new_traj.joint_trajectory = traj.joint_trajectory

    # 获取关节和轨迹点数量
    n_joints = len(traj.joint_trajectory.joint_names)
    n_points = len(traj.joint_trajectory.points)
    points = list(traj.joint_trajectory.points)
    
    # 调整时间、速度和加速度
    for i in range(n_points):
        point = JointTrajectoryPoint()
        point.positions = traj.joint_trajectory.points[i].positions
        point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
        point.velocities = list(traj.joint_trajectory.points[i].velocities)
        point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
        
        for j in range(n_joints):
            point.velocities[j] *= scale
            point.accelerations[j] *= scale * scale

        points[i] = point

    new_traj.joint_trajectory.points = points
    return new_traj

def execute_trajectory(waypoints, speed):
        global first_check, wait_sign
        flag = True
        level  = []
        level = waypoints[-4]
        # 初始化 moveit_commander 和 rospy
        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('move_group_python_interface', anonymous=True)
        arm_group = moveit_commander.MoveGroupCommander("manipulator")
        arm_group.set_named_target("middle")
        plan = arm_group.go(wait=True)
        arm_group.clear_pose_targets()
        rospy.Subscriber("/wrench_fake", WrenchStamped, wrench_callback)
        rospy.Subscriber('/joint_states', JointState, joint_state_callback)
        (plan, fraction) = arm_group.compute_cartesian_path(waypoints,0.005,False)
        print("Fraction of path achieved: ", fraction)
        # 调整轨迹速度
        if speed == 'fast':      new_plan = scale_trajectory_speed(plan, 1.5)
        elif speed == 'medium':  new_plan = scale_trajectory_speed(plan, 1.25)
        elif speed == 'slow':    new_plan = scale_trajectory_speed(plan, 1)
        arm_group.execute(new_plan, wait=False)

        while not rospy.is_shutdown():
            current_pose = arm_group.get_current_pose().pose
            if not first_check:
                if current_pose.position.z < 0.11:
                    first_check = True
            if first_check:
                waypoints = get_new_start_pose(current_pose, waypoints)
                (plan, fraction) = arm_group.compute_cartesian_path(waypoints,0.01,False)
                print("Fraction of path achieved: ", fraction)
                # 调整轨迹速度
                if speed == 'fast':      new_plan = scale_trajectory_speed(plan, 1.5)
                elif speed == 'medium':  new_plan = scale_trajectory_speed(plan, 1.25)
                elif speed == 'slow':    new_plan = scale_trajectory_speed(plan, 1)
                arm_group.execute(new_plan, wait=False)
                # 检查轨迹执行状态,力超出停止
                while not rospy.is_shutdown():
                    if not force_exceeded:
                        if wait_sign and flag:
                            print("wait")
                            arm_group.stop()
                            rospy.sleep(0.5)
                            waypoints = get_new_start_pose(current_pose, waypoints)
                            (plan, fraction) = arm_group.compute_cartesian_path(waypoints,0.01,False)
                            print("Fraction of path achieved: ", fraction)
                            flag = False
                            break
                        rospy.sleep(0.1)
                        current_pose = arm_group.get_current_pose().pose
                        if is_target_pose_reached(current_pose, waypoints[-1]):
                            break
                    else:
                        print("Stop trajectory...")
                        break
                arm_group.stop()
                #检查轨迹执行状态,力撤销继续
                while not rospy.is_shutdown():
                    if force_exceeded:
                        rospy.sleep(3)
                    else:
                        print("running...")
                        break
                # 清空目标位姿
                arm_group.clear_pose_targets()
                # 获取当前位姿并检查是否到达目标
                current_pose = arm_group.get_current_pose().pose
                # if wait_sign and flag:
                #     print("wait")
                #     arm_group.stop()
                #     rospy.sleep(5)
                #     waypoints = get_new_start_pose(current_pose, waypoints)
                #     (plan, fraction) = arm_group.compute_cartesian_path(waypoints,0.01,False)
                #     print("Fraction of path achieved: ", fraction)
                #     flag = False

                if is_target_pose_reached(current_pose, waypoints[-1]):
                    print("finish")
                    break
            
        arm_group.clear_pose_targets()
        rospy.sleep(3)
        arm_group.set_named_target("middle")
        arm_group.go(wait=True)

def trajectory_1():
    waypoints = []

    # 第1个目标位姿 14/-45/90/-45/0
    pose1 = geometry_msgs.msg.Pose()
    pose1.position.x = 0.1038815445000545
    pose1.position.y = 0.05599530875132761
    pose1.position.z = 0.287593164792294
    pose1.orientation.x = -7.443283672575919e-06
    pose1.orientation.y = 7.57226530269504e-05
    pose1.orientation.z = 0.12184966603307737
    pose1.orientation.w = 0.9925485646044253
    waypoints.append(pose1)

    # 第2个目标位姿 28/-45/90/-45/0
    pose2 = geometry_msgs.msg.Pose()
    pose2.position.x = 0.08365577107241515
    pose2.position.y = 0.1086784487646846
    pose2.position.z = 0.2876367813618629
    pose2.orientation.x = 1.760144636797022e-06
    pose2.orientation.y = 5.322223642645702e-07
    pose2.orientation.z = 0.24192347154193786
    pose2.orientation.w = 0.970295333346356
    waypoints.append(pose2)

    # 第3个目标位姿 28/-50/90/15/0
    pose3 = geometry_msgs.msg.Pose()
    pose3.position.x = -0.035022935822420206
    pose3.position.y = 0.045609591393929116
    pose3.position.z = 0.10616480515032783
    pose3.orientation.x = -0.11166965546205539
    pose3.orientation.y = 0.447983924420112
    pose3.orientation.z = 0.21466435979487009
    pose3.orientation.w = 0.8606738663070932
    waypoints.append(pose3)

    # 4 28/-70/120/-12/0
    pose4 = geometry_msgs.msg.Pose()
    pose4.position.x = 0.0059124806471319985
    pose4.position.y = 0.06736960543726173
    pose4.position.z = 0.0851790937420965
    pose4.orientation.x = -0.0787478196797887
    pose4.orientation.y = 0.3158430604116397
    pose4.orientation.z = 0.22880413581912243
    pose4.orientation.w = 0.9174369784990976
    waypoints.append(pose4)

    # 5 28/-65/121/-18/0
    pose5 = geometry_msgs.msg.Pose()
    pose5.position.x = 0.018667787233523553
    pose5.position.y = 0.0741529288356771
    pose5.position.z = 0.08468770404950043
    pose5.orientation.x = -0.07873418952429172
    pose5.orientation.y = 0.31589829849271545
    pose5.orientation.z = 0.22879950391761433
    pose5.orientation.w = 0.9174202850473789
    waypoints.append(pose5)

    # 第6个目标位姿 28/-68/128/-27/0
    pose6 = geometry_msgs.msg.Pose()
    pose6.position.x = 0.032490128976992244
    pose6.position.y = 0.08148829798108001
    pose6.position.z = 0.08403494444521113
    pose6.orientation.x = -0.0686576561182485
    pose6.orientation.y = 0.275514230486597
    pose6.orientation.z = 0.2319904963985905
    pose6.orientation.w = 0.9303539351432134
    waypoints.append(pose6)

    # 第7个目标位姿 28/-70/135/-36/0
    pose7 = geometry_msgs.msg.Pose()
    pose7.position.x = 0.044606693998343105
    pose7.position.y = 0.08792626579373217
    pose7.position.z = 0.08060679535604816
    pose7.orientation.x = -0.06055302337925643
    pose7.orientation.y = 0.24294582246096078
    pose7.orientation.z = 0.23423143748854183
    pose7.orientation.w = 0.9393861252969676
    waypoints.append(pose7)

    # 第8个目标位姿 28/-70/141/-50/0
    pose8 = geometry_msgs.msg.Pose()
    pose8.position.x = 0.06420066898792094
    pose8.position.y = 0.098369914979654
    pose8.position.z = 0.09411826462949197
    pose8.orientation.x = -0.044063328860029054
    pose8.orientation.y = 0.17674516776628177
    pose8.orientation.z = 0.2379435127905711
    pose8.orientation.w = 0.9540453099522713
    waypoints.append(pose8)

    # 第9个目标位姿 28/72/150/-78/0 举平
    pose9 = geometry_msgs.msg.Pose()
    pose9.position.x = 0.08723638964773509
    pose9.position.y = 0.11059034428259178
    pose9.position.z = 0.1531080334994858
    pose9.orientation.x = 3.17781117137612e-05
    pose9.orientation.y = -0.00011987965642704671
    pose9.orientation.z = 0.24193188893662781
    pose9.orientation.w = 0.970293226676645
    waypoints.append(pose9)


    # # 第10个目标位姿 14/72/150/-78/0 举平移
    pose10 = geometry_msgs.msg.Pose()
    pose10.position.x = 0.10781150531469692
    pose10.position.y = 0.057003964556817374
    pose10.position.z = 0.1530723139619223
    pose10.orientation.x = 9.186180341598707e-06
    pose10.orientation.y = -5.977016834688678e-05
    pose10.orientation.z = 0.12192029901858292
    pose10.orientation.w = 0.9925398919088142
    waypoints.append(pose10)

    # 第11个目标位姿 middle
    pose11 = geometry_msgs.msg.Pose()
    pose11.position.x = 0.11075168589927481
    pose11.position.y = -1.091277667118024e-05
    pose11.position.z = 0.2875988062936282
    pose11.orientation.x = 1.8382147442206865e-06
    pose11.orientation.y = 8.132110012412454e-05
    pose11.orientation.z = -2.4453694472133078e-05
    pose11.orientation.w = 0.9999999963927584
    waypoints.append(pose11)

    # 第12个目标位姿 0/40/0/-40/0
    pose12 = geometry_msgs.msg.Pose()
    pose12.position.x = 0.3035704582748756
    pose12.position.y = -4.6019088696090045e-05
    pose12.position.z = 0.30536264212765296
    pose12.orientation.x = 1.8345570569793093e-06
    pose12.orientation.y = -7.180863568924419e-05
    pose12.orientation.z = -6.589662785214818e-05
    pose12.orientation.w = 0.9999999952488945
    waypoints.append(pose12)


    return waypoints

def trajectory_2():
    waypoints = []

    # 第1个目标位姿 0/-50/90/15/0
    pose1 = geometry_msgs.msg.Pose()
    pose1.position.x = -0.023623156207669485
    pose1.position.y = -5.61926355873778e-06
    pose1.position.z = 0.1061805918499025
    pose1.orientation.x = -2.4631350004671237e-05
    pose1.orientation.y = 0.4616371139275134
    pose1.orientation.z = -7.937233310081275e-06
    pose1.orientation.w = 0.8870688667600578
    waypoints.append(pose1)

    # 2 0/-70/120/-12/0
    pose2 = geometry_msgs.msg.Pose()
    pose2.position.x = 0.02270275459045043
    pose2.position.y = -3.0206178897082266e-05
    pose2.position.z = 0.08515341454770431
    pose2.orientation.x = -3.0504780108955957e-05
    pose2.orientation.y = 0.32557952141613367
    pose2.orientation.z = -8.924993019861189e-05
    pose2.orientation.w = 0.9455146568606697
    waypoints.append(pose2)

    # 3 0/-65/121/-18/0
    pose3 = geometry_msgs.msg.Pose()
    pose3.position.x = 0.03716382693672583
    pose3.position.y = -1.5665732298609626e-05
    pose3.position.z = 0.08467326062547971
    pose3.orientation.x = -2.6268154351170503e-05
    pose3.orientation.y = 0.3256253211597455
    pose3.orientation.z = -4.6484770819418694e-05
    pose3.orientation.w = 0.945498888084361
    waypoints.append(pose3)

    # 第4个目标位姿 0/-68/128/-27/0
    pose4 = geometry_msgs.msg.Pose()
    pose4.position.x = 0.05279465803518045
    pose4.position.y = 2.85229202356785e-05
    pose4.position.z = 0.08403403724734092
    pose4.orientation.x = -1.0133215588657257e-05
    pose4.orientation.y = 0.2839328542037646
    pose4.orientation.z = 7.950130493131293e-05
    pose4.orientation.w = 0.9588441624584073
    waypoints.append(pose4)

    # 第5个目标位姿 0/-70/135/-33/0
    pose5 = geometry_msgs.msg.Pose()
    pose5.position.x = 0.059453244311609865
    pose5.position.y = -8.330909781430266e-06
    pose5.position.z = 0.07075068945087021
    pose5.orientation.x = -2.782322372343405e-06
    pose5.orientation.y = 0.2755682005866627
    pose5.orientation.z = -2.508720262445286e-05
    pose5.orientation.w = 0.9612815228580647
    waypoints.append(pose5)

    # 第6个目标位姿 0/-70/141/-50/0
    pose6 = geometry_msgs.msg.Pose()
    pose6.position.x = 0.08872055239306093
    pose6.position.y = 0.00012314349764190854
    pose6.position.z = 0.09403262420395148
    pose6.orientation.x = -1.2451783763174253e-05
    pose6.orientation.y = 0.1823097069171027
    pose6.orientation.z = 0.0003162268136817005
    pose6.orientation.w = 0.9832411050242742
    waypoints.append(pose6)

    # # 第7个目标位姿 0/-72/150/-78/0 举平
    #0/-72/142/-78/0
    pose7 = geometry_msgs.msg.Pose()
    pose7.position.x = 0.11482399330409013
    pose7.position.y = 2.1035317438294303e-05
    pose7.position.z = 0.1529639014580961
    pose7.orientation.x = 1.8613930079306421e-06
    pose7.orientation.y = 0.0001556714793681703
    pose7.orientation.z = 8.278564739240959e-05
    pose7.orientation.w = 0.9999999844547312
    waypoints.append(pose7)

    # # 第9个目标位姿 16/-72/150/-78/0 举平移
    #16/-75/142/-80/0
    pose9 = geometry_msgs.msg.Pose()
    pose9.position.x = 0.10568869680086797
    pose9.position.y = 0.06496941623114455
    pose9.position.z = 0.1529569397066705
    pose9.orientation.x = -2.5174032480959837e-05
    pose9.orientation.y = 0.0001925602901853463
    pose9.orientation.z = 0.13929909717987762
    pose9.orientation.w = 0.9902503339114176
    waypoints.append(pose9)

    # 第10个目标位姿 middle
    pose10 = geometry_msgs.msg.Pose()
    pose10.position.x = 0.11075168589927481
    pose10.position.y = -1.091277667118024e-05
    pose10.position.z = 0.2875988062936282
    pose10.orientation.x = 1.8382147442206865e-06
    pose10.orientation.y = 8.132110012412454e-05
    pose10.orientation.z = -2.4453694472133078e-05
    pose10.orientation.w = 0.9999999963927584
    waypoints.append(pose10)

    # 第11个目标位姿 0/40/0/-40/0
    pose11 = geometry_msgs.msg.Pose()
    pose11.position.x = 0.3035704582748756
    pose11.position.y = -4.6019088696090045e-05
    pose11.position.z = 0.30536264212765296
    pose11.orientation.x = 1.8345570569793093e-06
    pose11.orientation.y = -7.180863568924419e-05
    pose11.orientation.z = -6.589662785214818e-05
    pose11.orientation.w = 0.9999999952488945
    waypoints.append(pose11)

    return waypoints

def trajectory_3():
    waypoints = []

   # 第1个目标位姿 -14/-45/90/-45/0
    pose1 = geometry_msgs.msg.Pose()
    pose1.position.x = 0.1038794732882471
    pose1.position.y = -0.055993214192435026
    pose1.position.z = 0.2876041751758597
    pose1.orientation.x = 7.895216365982998e-06
    pose1.orientation.y = 4.925607556231088e-05
    pose1.orientation.z = -0.12184804269604492
    pose1.orientation.w = 0.9925487655539387
    waypoints.append(pose1)

    # 第2个目标位姿 -28/-45/90/-45/0
    pose2 = geometry_msgs.msg.Pose()
    pose2.position.x = 0.08367758643029065
    pose2.position.y = -0.1086808498137246
    pose2.position.z = 0.2875887574546225
    pose2.orientation.x = 1.927121538709558e-05
    pose2.orientation.y = 6.971049619118313e-05
    pose2.orientation.z = -0.24189446329796155
    pose2.orientation.w = 0.97030256280959
    waypoints.append(pose2)

    # 第3个目标位姿 -28/-50/90/15/0
    pose3 = geometry_msgs.msg.Pose()
    pose3.position.x = -0.03502840972419606
    pose3.position.y = -0.045589483814465884
    pose3.position.z = 0.10614045818885712
    pose3.orientation.x = 0.1116759438658971
    pose3.orientation.y = 0.44806053427619247
    pose3.orientation.z = -0.21459909236682168
    pose3.orientation.w = 0.8606494470695548
    waypoints.append(pose3)

    # 4 -28/-70/120/-12/0
    pose4 = geometry_msgs.msg.Pose()
    pose4.position.x = 0.005919275733848872
    pose4.position.y = -0.067335769649617
    pose4.position.z = 0.08514319292225526
    pose4.orientation.x = 0.07874286740695134
    pose4.orientation.y = 0.31590177237344613
    pose4.orientation.z = -0.22869782778735018
    pose4.orientation.w = 0.9174436956070898
    waypoints.append(pose4)

    # 5 -28/-65/121/-17/0
    pose5 = geometry_msgs.msg.Pose()
    pose5.position.x = 0.016198825353478746
    pose5.position.y = -0.07282416841740312
    pose5.position.z = 0.08179027140107833
    pose5.orientation.x = 0.08069348056375333
    pose5.orientation.y = 0.3238641972342192
    pose5.orientation.z = -0.22806551965504196
    pose5.orientation.w = 0.9146511152832089
    waypoints.append(pose5)

    # 第6个目标位姿 -28/-68/128/-27/0
    pose6 = geometry_msgs.msg.Pose()
    pose6.position.x = 0.032492179697122675
    pose6.position.y = -0.0814728216582047
    pose6.position.z = 0.08401846064911692
    pose6.orientation.x = 0.06864384957945104
    pose6.orientation.y = 0.27555892205004473
    pose6.orientation.z = -0.23195179933670168
    pose6.orientation.w = 0.9303513665158974
    waypoints.append(pose6)

    # 第7个目标位姿 -28/-70/135/-36/0
    pose7 = geometry_msgs.msg.Pose()
    pose7.position.x = 0.0446287643769399
    pose7.position.y = -0.08791412462470531
    pose7.position.z = 0.08061218194929645
    pose7.orientation.x = 0.06051325339911822
    pose7.orientation.y = 0.242909963020376
    pose7.orientation.z = -0.23418814928531106
    pose7.orientation.w = 0.9394087538248804
    waypoints.append(pose7)


    # 第8个目标位姿 -28/-70/141/-50/0
    pose8 = geometry_msgs.msg.Pose()
    pose8.position.x = 0.06423452902701184
    pose8.position.y = -0.09829584973459393
    pose8.position.z = 0.09410037803385392
    pose8.orientation.x = 0.04403927409919545
    pose8.orientation.y = 0.17677706686087613
    pose8.orientation.z = -0.23775784600096436
    pose8.orientation.w = 0.9540867977463388
    waypoints.append(pose8)

    # 第9个目标位姿 -28/-72/150/-78/0 举平
    pose9 = geometry_msgs.msg.Pose()
    pose9.position.x = 0.0872709979971224
    pose9.position.y = -0.11052866737323061
    pose9.position.z = 0.1530968348692439
    pose9.orientation.x = -2.2012373911066797e-05
    pose9.orientation.y = -9.595675516130216e-05
    pose9.orientation.z = -0.2417885076640756
    pose9.orientation.w = 0.9703289688911364
    waypoints.append(pose9)

    # # 第9.5个目标位姿 -28/-69/154/-90/0 举平
    # pose99 = geometry_msgs.msg.Pose()
    # pose99.position.x = 0.09283120272437292
    # pose99.position.y = -0.11345799827805057
    # pose99.position.z = 0.16258990409537252
    # pose99.orientation.x = -0.01053572946781694
    # pose99.orientation.y = -0.042308901440091425
    # pose99.orientation.z = -0.24151692254776375
    # pose99.orientation.w = 0.9694165933109311
    # waypoints.append(pose99)

    # 第10个目标位姿 -14/-72/150/-78/0 举平移
    pose10 = geometry_msgs.msg.Pose()
    pose10.position.x = 0.1078137015424794
    pose10.position.y = -0.057002013797603834
    pose10.position.z = 0.15304097497404734
    pose10.orientation.x = 7.721304036228014e-06
    pose10.orientation.y = 4.7830876074295094e-05
    pose10.orientation.z = -0.12190999444880327
    pose10.orientation.w = 0.9925411582932376
    waypoints.append(pose10)

    # 第11个目标位姿 middle
    pose11 = geometry_msgs.msg.Pose()
    pose11.position.x = 0.11075168589927481
    pose11.position.y = -1.091277667118024e-05
    pose11.position.z = 0.2875988062936282
    pose11.orientation.x = 1.8382147442206865e-06
    pose11.orientation.y = 8.132110012412454e-05
    pose11.orientation.z = -2.4453694472133078e-05
    pose11.orientation.w = 0.9999999963927584
    waypoints.append(pose11)

    # 第12个目标位姿 0/40/0/-40/0
    pose12 = geometry_msgs.msg.Pose()
    pose12.position.x = 0.3035704582748756
    pose12.position.y = -4.6019088696090045e-05
    pose12.position.z = 0.30536264212765296
    pose12.orientation.x = 1.8345570569793093e-06
    pose12.orientation.y = -7.180863568924419e-05
    pose12.orientation.z = -6.589662785214818e-05
    pose12.orientation.w = 0.9999999952488945
    waypoints.append(pose12)

    return waypoints

def run_selected_trajectory(traj_index, speed):
    if traj_index == 1:
        waypoints = trajectory_1()
    elif traj_index == 2:
        waypoints = trajectory_2()
    elif traj_index == 3:
        waypoints = trajectory_3()
    else:
        print("无效的轨迹编号")
        return

    if speed not in ['fast', 'medium', 'slow']:
        print("无效的速度。请选择 'fast', 'medium' 或 'slow'.")
        return
    execute_trajectory(waypoints, speed)

if __name__ == '__main__':
    try:
        # 示例: 执行第二条轨迹的中速
        # rospy.Subscriber("/wrench_fake", WrenchStamped, wrench_callback)
        # rospy.Subscriber('/joint_states', JointState, joint_state_callback)
        run_selected_trajectory(1, 'slow')
        run_selected_trajectory(2, 'medium')
        # run_selected_trajectory(3, 'fast')
    except rospy.ROSInterruptException:
        pass
